#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S2,     HTSMUX,         sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     Wheel_Right,   tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     Wheel_Left,    tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    ser7vo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "drivers/HTSMUX-driver.h"
#include "drivers/HTIRS2-driver.h"

const tMUXSensor HTIRS2 = msensor_S2_1;

task main ()
{
  int _dirDC = 0;
  int _dirAC = 0;
  int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
  int acS1, acS2, acS3, acS4, acS5 = 0;

  while (true)
  {
    _dirDC = HTIRS2readDCDir(HTIRS2);
    if (_dirDC < 0)
      break;
    _dirAC = HTIRS2readACDir(HTIRS2);
    if (_dirAC < 0)
      break;
    if (!HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5))
      break;
    if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ))
      break;

    if(acS4 > 31 && acS4 < 34)
    {
      eraseDisplay();
      nxtDisplayTextLine(3, "Left");
      wait1Msec(10000);
    }
    if(acS4 > 48 && acS4 < 41)
    {
      eraseDisplay();
      nxtDisplayTextLine(3, "Center");
      wait1Msec(10000);
    }
    if(acS4 > 24 && acS4 < 27)
    {
      eraseDisplay();
      nxtDisplayTextLine(3, "Right");
      wait1Msec(10000);
    }
    nxtDisplayTextLine(3, "IR Not Found!");
  }
}
